#pragma once

#include <navigation/srv/rviz2_nav.hpp>
#include <navigation/deviation_controller.hpp>
#include <navigation/state_manager.hpp>
#include <rclcpp/rclcpp.hpp>
#include <navigation/rviz2_gui/rviz2_const.hpp>
#include <std_msgs/msg/bool.hpp>
#include <thread>

using navigation::srv::Rviz2Nav;

class Rviz2Connecter {
public:
    Rviz2Connecter(
        std::shared_ptr<rclcpp::Node> node,
        std::shared_ptr<StateManager> state_manager,
        std::shared_ptr<DeviationBaseController> deviation_controller
    ):  node_(node),
        state_manager_(state_manager),
        deviation_controller_(deviation_controller)
    {
        controller_is_paused_pub_ = node_->create_publisher<std_msgs::msg::Bool>("controller_is_paused", 1);
        controller_is_paused_pub_thread = std::thread([this]() {
            rclcpp::Rate rate(10.0);
            std_msgs::msg::Bool msg;
            while (rclcpp::ok()) {
                msg.data = deviation_controller_->is_paused_();
                controller_is_paused_pub_->publish(msg);
                rate.sleep();
            }
        });
        service_ = node_->create_service<Rviz2Nav>(
            "rviz2_nav",
            std::bind(&Rviz2Connecter::handle_service, this, std::placeholders::_1, std::placeholders::_2)
        );
    }

private:
    void handle_service(
        const std::shared_ptr<Rviz2Nav::Request> request,
        std::shared_ptr<Rviz2Nav::Response> response
    ) {
        Rviz2ConnecterType type = static_cast<Rviz2ConnecterType>(request->type);
        response->success = true;
        switch (type) {
            case Rviz2ConnecterType::START:
                deviation_controller_->start();
                break;
            case Rviz2ConnecterType::PAUSE:
                deviation_controller_->pause();
                break;
            case Rviz2ConnecterType::ADD_TARGET: {
                geometry_msgs::msg::Pose target = request->target;
                state_manager_->add_target(
                    target.position.x,
                    target.position.y,
                    target.position.z,
                    target.orientation.x,
                    target.orientation.y,
                    target.orientation.z,
                    target.orientation.w,
                    -1  // 添加到最后
                );
                break;
            }
            case Rviz2ConnecterType::REMOVE_TARGET: {
                int index = request->index1;
                state_manager_->remove_target(index);
                break;
            }
            case Rviz2ConnecterType::MOVE_TARGET: {
                int index1 = request->index1;
                int index2 = request->index2;
                state_manager_->move_target(index1, index2);
                break;
            }
            case Rviz2ConnecterType::HIGHLIGHT_TARGETS: {
                std::vector<int> indices = request->highlighted_indices;
                state_manager_->set_highlight_targets(indices);
                break;
            }
            case Rviz2ConnecterType::SAVE_TARGETS: {
                std::string file_name = request->message;
                std::tie(response->success, response->message) = state_manager_->save_targets_to_file(file_name);
                break;
            }
            case Rviz2ConnecterType::LOAD_TARGETS: {
                std::string file_name = request->message;
                std::tie(response->success, response->message) = state_manager_->load_targets_from_file(file_name);
                break;
            }
            case Rviz2ConnecterType::GET_MAPS_DIR_PATH: {
                response->message = state_manager_->get_maps_dir_path();
                break;
            }
            default:
                response->success = false;
                response->message = "Unknown service type.";
        }
    }

    // Initialization
    std::shared_ptr<rclcpp::Node> node_;
    std::shared_ptr<StateManager> state_manager_;
    std::shared_ptr<DeviationBaseController> deviation_controller_;

    std::shared_ptr<rclcpp::Service<Rviz2Nav>> service_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr controller_is_paused_pub_;
    std::thread controller_is_paused_pub_thread;
};
